// File:          my_controller.cpp
// Date:
// Description:
// Author:  baolan
// Modifications:

// You may need to add webots include files such as
// <webots/DistanceSensor.hpp>, <webots/Motor.hpp>, etc.
// and/or to add some other includes
#include <motionControl.hpp>
#include <robot_locomotion.h>
#include <ctime>
#include <fstream>
#define PI 3.1415926

// All the webots classes are defined in the "webots" namespace
using namespace webots;
MotionControl mc;
rl rl1;

int main(int argc, char **argv) {
    Robot *robot = new Robot();
    Keyboard kb;
    kb.enable(1000);
    float timePeriod = 0.01;
    mc.MotionContr(timePeriod, robot);

    Camera *camera;
    camera=robot->getCamera("camera");
    camera->enable(1000);
    int timeStep = (int)robot->getBasicTimeStep();

    Matrix<float, 4, 3> initPos;
    initPos<<  0.0, 0.0, -0.23, 0.0, 0.0, -0.23, 0.0, 0.0, -0.23, 0.0, 0.0, -0.23;
    mc.setInitPos(initPos);
    mc.tCV<< 0.00, 0.0, 0.0;
    mc.swingFlag = 0;
    //w:87,s:83,a:65,d:68，q:81,e:69
    mc.setCoMVel(mc.tCV);
    char StateVal;
    StateVal = mc.stateval[0];

    ofstream oFile;
    oFile.open("distance.csv",ios::out | ios::trunc);
  while (robot->step(timeStep) != -1)
  {
    mc.key = kb.getKey();
    mc.Sensor_update();
    mc.forwardKinematics();
    mc.jacobians();
    mc.state_judgement();

    rl1.dsensor[0] =  mc.distance[0];
    rl1.dsensor[1] =  mc.distance[1];
    rl1.dsensor[2] =  mc.distance[2];
    rl1.dsensor[3] =  mc.distance[3];
    rl1.dsensor[4] =  mc.distance[4];
    rl1.dsensor[5] =  mc.distance[5];

    rl1.run();
    rl1.update();
    
    // mc.tCV[0] = rl1.vx;
    // mc.tCV[1] = rl1.vy;
    // mc.tCV[2] = rl1.w;
    switch(StateVal)
    {
      case 'a'://
         mc.State_start();
         if (mc.Start_time >= 0.80)
         {
           StateVal = mc.stateval[1];
           mc.fly_time = 0.0;
         }
         cout << "a"<<endl;
         mc.X <<  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -9.81;
         mc.mark_xyz = mc.global_xyz;
         cout << "gps:  "<< mc.global_xyz.transpose()<<endl;
         break;
    
      case 'b':// 2 leg stance
        mc.present_pxyz();
        mc.stance_MPC();
        mc.swing_VMC();
        mc.Setjoint();
        cout << "b"<< endl;
        break;
      case 'c'://3 leg stance
        break;
      case 'd'://4 leg stance
        break;
      default:
      break;
    }
    cout << "v:   "<<mc.tCV.transpose()<< "  "<< mc.High<<endl;
    // cout << "imu:   "<< mc.imu_num.transpose()<<" "<< mc.target_taoz<<endl;
    // cout << "dis: "<<mc.distance.transpose()<<endl;
    // oFile << mc.distance[0] << "," << mc.distance[1] << "," << mc.distance[2] << "," << mc.distance[3] << "," <<  mc.distance[4] <<"," << mc.distance[5] << endl;
  }
oFile.close();
delete robot;
return 0;
}
